 /***************************************************************************************
 * Copyright 2020 Infineon Technologies AG ( www.infineon.com ).                       *
 * All rights reserved.                                                                *
 *                                                                                     *
 * Licensed  Material-Property of Infineon Technologies AG.                            *
 * This software is made available solely pursuant to the terms of Infineon            *
 * Technologies AG agreement which governs its use. This code and the information      *
 * contained in it are proprietary and confidential to Infineon Technologies AG.       *
 * No person is allowed to copy, reprint, reproduce or publish any part of this code,  *
 * nor disclose its contents to others, nor make any use of it, nor allow or assist    *
 * others to make any use of it - unless by prior Written express authorization of     *
 * Infineon Technologies AG and then only to the extent authorized.                    *
 *                                                                                     *
 * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,            *
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY,           *
 * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT, ARE DISCLAIMED.  IN NO       *
 * EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,     *
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,                 *
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;         *
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY             *
 * WHETHER IN  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR            *
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF              *
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                          *
 *                                                                                     *
 ***************************************************************************************/
/**
 * @file   helper.c
 * @date   June, 2020
 * @brief  Implementation of common related platform functions
 */
#include <string.h>
#include <stdio.h>
#include "stdarg.h"
#include <stdlib.h>

#include "register.h"
#include "auths_api.h"
#include "auths_config.h"
#include "auths_status.h"
#include "helper.h"

#include "i2c_bus.h"
#include "device.h"

//todo System porting required: implements platform dependency header for HW RNG, CRC, SHA, timer or GPIO, if required.

extern AuthS_Capability auths_capability;
extern unsigned int actual_degree;
/**
 * @brief Print project related info
  * */
void print_info(void){
	uint16_t ret;
	PRINT("===================\r\n");
	PRINT("OPTIGA(TM) Authenticate S 0x1x SDK Example\r\n");

#if (SHOW_SDK_VERSION==1)
	uint16_t version;
	uint32_t date;
	ret = auths_get_sdk_version(&version, &date);
	if (SDK_SUCCESS != ret) {
		PRINT("Error: Get SDK version ret=0x%X\r\n", ret);
	} else {
		PRINT("Compile: %s:%s \r\nVersion: %X Release Date: %X\r\n", __DATE__, __TIME__, version, date);
	}
#endif

#if (SHOW_CRYPTO_LIB_VERSION==1)
	uint16_t crypto_version;
	uint32_t crypto_date;
	ret = auths_get_crypto_version(&crypto_version, &crypto_date);
	if(TRUE!= ret){
		PRINT("Error: Get Crypto library version, ret=0x%X\r\n", ret);
	}else {
		PRINT("Crypto library version: %X Release Date: %X\r\n",crypto_version, crypto_date);
	}
	PRINT("===================\r\n\n");
#endif
}


/**
 * @brief MCU SHA256
 * @param in Message input
 * @param out Output hash value
 * @param length input message length
  \todo System porting required: if target MCU supports sha256, provide a callback to this function. Otherwise, remove code.
 * */
void hw_sha256(uint8_t *in, uint8_t *out, const uint32_t len) {
//todo System porting required: implements HW SHA, if required.
}

/**
 * @brief MCU random number generator
 * @param random_byte Pointer to random byte output
 * @param len Random byte length
 \todo System porting required: if target MCU supports RNG, provide a callback to this function. Otherwise, remove code.
 * */
void hw_rng (uint8_t* random_byte, uint8_t len){
//todo System porting required: implements HW SHA, if required.
}

/** !!!!!!!!!! W A R N I N G !!!!!!!!!!
  *
  * this implementation calls the system function rand() for random number generation.
  * rand() is not a cryptographically strong random number generator.
  *
  * !!!!! this implementation must be replaced by a program using an unpredictable
  * true or pseudo random number generator with good statistical properties !!!!!
  */

/**
* @brief generates a 128, 160, or 192 bit random number based on the vector size
* @param erg generated random number
*/
int gf2n_rand(uint32_t *  erg){
  int i;
  memset(erg, 0, ARRAY_LEN(MAX_DEGREE) * 4);

  if(auths_capability.auths_alt_rng!=NULL){
	  auths_capability.auths_alt_rng((uint8_t *)erg, (ARRAY_LEN(actual_degree))*4);
  } else{

    for (i = 0; i < ARRAY_LEN(actual_degree)-1; i++){
        erg[i] = (uint32_t)rand();
    }  
  }
  return FALSE;
}

/**
* @brief generates a random number based on the require length
* @param ub_rand generated random number
* @param ub_rand_len length of generated random number
*/
void rand_byte(uint8_t *ub_rand, uint8_t ub_rand_len){    
	uint8_t i;
    if(auths_capability.auths_alt_rng!=NULL){
    	auths_capability.auths_alt_rng(ub_rand, ub_rand_len);
    } else{
	    for (i = 0; i < ub_rand_len; i++){
	        ub_rand[i] = (uint8_t)rand();
        }
    }
}


/** \brief definition of sha256 context */
typedef struct{
  uint32_t H[8];      /*!< 8 working variables/final hash value */
  uint32_t length;    /*!< length counter */
  unsigned int next;  /*!< next unused byte in message buffer */
  uint8_t M[64];      /*!< message buffer */
} sha256_context_t;

static void sha256_init(sha256_context_t *context);
static void sha256_update(const uint8_t *input_data, const uint32_t input_length, sha256_context_t *context);
static void sha256_final(uint8_t *hash_value, sha256_context_t *context);

/** shift right operation */
#define SR(A, B) ((A)>>(B))

/** rotate right operation */
#define ROTR(A, B) (((A)>>(B))|((A)<<(32-(B))))

/** rotate left operation */
#define ROTL(A, B) (((A)<<(B))|((A)>>(32-(B))))

#define CH(x, y, z) ((z) ^ ((x) & ((y) ^ (z))))
#define MAJ(x, y, z) (((x) & ((y) ^ (z))) ^ ((y) & (z)))
#define BIG_SIG_0(x) ((ROTR((x), 2) ^ ROTR((x), 13)) ^ ROTR((x), 22))
#define BIG_SIG_1(x) (ROTR((x), 6) ^ ROTR((x), 11) ^ ROTR((x), 25))
#define LIT_SIG_0(x) (ROTR((x), 7) ^ ROTR((x), 18) ^ SR((x), 3))
#define LIT_SIG_1(x) (ROTR((x), 17) ^ ROTR((x), 19) ^ SR((x), 10))

static const uint32_t K[64] = {
  0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5,
  0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174,
  0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da,
  0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7, 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967,
  0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85,
  0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070,
  0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3,
  0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2};

/*---------------------------------------------------------------------------*/
/** \brief sha256 compression function
  * \param[in,out] context pointer to sha256 context
  */
static void sha256_compress(sha256_context_t *context) {
  uint32_t a, b, c, d, e, f, g, h, i, t1, t2;
  uint32_t W[16];

  for (i = 0; i < 16; i++) W[i] = BIG_U8TO32(&(context->M[i<<2]));

  a = context->H[0];
  b = context->H[1];
  c = context->H[2];
  d = context->H[3];
  e = context->H[4];
  f = context->H[5];
  g = context->H[6];
  h = context->H[7];

  for (i = 0; i < 64; i++) {
    if (i > 15) W[i & 15] = LIT_SIG_1(W[(i-2) & 15])+W[(i-7) & 15]+LIT_SIG_0(W[(i-15) & 15])+W[(i-16) & 15];
    t1 = h+BIG_SIG_1(e)+CH(e, f, g)+K[i]+W[i & 15];
    t2 = BIG_SIG_0(a)+MAJ(a, b, c);
    h = g;
    g = f;
    f = e;
    e = d+t1;
    d = c;
    c = b;
    b = a;
    a = t1+t2;
  }

  context->H[0] += a;
  context->H[1] += b;
  context->H[2] += c;
  context->H[3] += d;
  context->H[4] += e;
  context->H[5] += f;
  context->H[6] += g;
  context->H[7] += h;

  context->next = 0;
}

/*---------------------------------------------------------------------------*/
/** \brief sha256 context initialization
  * \param[in,out] context pointer to sha256 context
  */
static void sha256_init(sha256_context_t *context) {
  context->length = 0;
  context->next = 0;
  context->H[0] = 0x6a09e667;
  context->H[1] = 0xbb67ae85;
  context->H[2] = 0x3c6ef372;
  context->H[3] = 0xa54ff53a;
  context->H[4] = 0x510e527f;
  context->H[5] = 0x9b05688c;
  context->H[6] = 0x1f83d9ab;
  context->H[7] = 0x5be0cd19;
}

/*---------------------------------------------------------------------------*/
/** \brief sha256 update function
  * \param[in] input_data pointer to input data
  * \param[in] input_length byte length of input data
  * \param[in,out] context pointer to sha256 context
  */
static void sha256_update(const uint8_t *input_data, const uint32_t input_length, sha256_context_t *context) {
  uint32_t i;

  for (i = 0; i < input_length; i++) {
    context->M[context->next] = input_data[i];
    context->next++;
    if (context->next == 64) sha256_compress(context);
  }
  context->length += (input_length<<3);
}

/*---------------------------------------------------------------------------*/
/** \brief finalize sha256 hashing
  * \param[out] hash_value pointer to buffer
  * \param[in,out] context pointer to sha256 context
  */
static void sha256_final(uint8_t *hash_value, sha256_context_t *context) {
  uint8_t i;

  i = context->next;
  context->M[i] = 128;
  i++;
  if (i > 56) {
    for (; i < 64; i++) context->M[i] = 0;
    sha256_compress(context);
    i = 0;
  }
  for (; i < 60; i++) context->M[i] = 0;
  BIG_U32TO8((context->M)+60, context->length);
  sha256_compress(context);

  for (i = 0; i < 8; i++) BIG_U32TO8(hash_value+4*i, context->H[i]);
}

/*---------------------------------------------------------------------------*/
/** \brief sha256 hashing in one call
  * \param[out] hash_value pointer to hash output
  * \param[in] input_data pointer to data to hash
  * \param[in] input_length byte length of hash data
  */
void sha256(uint8_t *hash_value, const uint8_t *input_data, const uint32_t input_length) {
  sha256_context_t ctx;

  sha256_init(&ctx);
  sha256_update(input_data, input_length, &ctx);
  sha256_final(hash_value, &ctx);
}

/**
 * @brief Loop counter to implement delay which will affect global timing.
 * param ul_ticks counter
 * \todo System porting required: Modify this function for GPIO bit banging operation.
 */
void delay_us(volatile uint32_t ul_ticks) {
	// for (; ul_ticks; ul_ticks--)
	// 	;
  Device_DelayUs(ul_ticks);
} 
/**
* @brief System timer delay in ms
* @param uw_time_ms delay in ms
\todo System porting required: implements delay according to platform
*/
void delay_ms(uint32_t uw_time_ms) {
//todo System porting required: implements HW timer, if required.
  //   for(;uw_time_ms;uw_time_ms--){
	// 	delay_us(1000);
	// }
  Device_DelayMs(uw_time_ms);
}


//todo System porting required: implements hardware CRC according to platform. If not supported, disable it.
static uint8_t Reflect8(uint8_t in){
    uint8_t resByte = 0;

    for (int i = 0; i < 8; i++){
        if ((in & (1 << i)) != 0){
            resByte |= (uint8_t)(1 << (7 - i));
        }
    }

    return resByte;
}

static uint16_t Reflect16(uint16_t val){
    uint16_t resVal = 0;

    for (int i = 0; i < 16; i++){
        if ((val & (1 << i)) != 0){
            resVal |= (uint16_t)(1 << (15 - i));
        }
    }

    return resVal;
}

/**
* @brief Generates CRC required for I2C 
* @param data_p data input to be calculated
* @param len number of bytes 
*/
uint16_t crc16_gen(uint8_t *data_p, uint16_t len){
     uint16_t i;
     //uint16_t data;
     uint16_t crc = 0xffff;
	 uint8_t data_reflet;

	if (len == 0)
	return (~crc);

	for(i=0; i<len; i++){
	    data_reflet = Reflect8(data_p[i]);
        crc ^= data_reflet << 8; /* move byte into MSB of 16bit CRC */

        for (int j = 0; j < 8; j++){
            if ((crc & 0x8000) != 0){ /* test for MSB = bit 15 */            
                crc = ((crc << 1) ^ POLY);
            }
            else{
                crc <<= 1;
            }
        }
    }
	crc = Reflect16(crc);
    crc = crc ^ 0xffff;

	return (crc);
}

#if (ENABLE_DEBUG_PRINT == 1)
void PrintByteArrayHex(uint8_t *data, uint16_t ArrayLen){
	uint16_t i;
	for(i=0; i<ArrayLen; i++)
	{
		if((i != 0 )&&((i & 0x07) ==0) && (ArrayLen > 15 )){
            PRINT("\r\n");
        }
		PRINT("0x%.2X ", data[i]);
	}
	PRINT("\r\n");
}
/**
* @brief Prints a selected row of the UID table structure
*/
void print_row_UID_table(uint8_t *UID) {
     PRINT("UID: ");
    for (uint8_t i = 0; i < 12; i++) {
        PRINT("%.2x ", UID[i]);
    }
    PRINT("\r\n");
}

/**
* @brief Prints ODC
*/
void print_odc(uint32_t *ODC){
PRINT("ODC:\r\n");
    for (int i = 0; i < 12; i++) {
        PRINT("%.8x ", ODC[i]);
        if(i==0){
            continue;
        }
        if(((i+1)%4)==0){
            PRINT("\r\n");
        }
    }
    PRINT("\r\n");
}

/**
* @brief Prints Public key
*/
void print_puk(uint32_t *PUK){
    PRINT("PUK:\r\n");
    for (int i = 0; i < 5; i++) {
        PRINT("%.8x ", PUK[i]);
        if (i == 0) {
            continue;
        }
        if (((i + 1) % 4) == 0) {
            PRINT("\r\n");
        }
    }
    PRINT("\r\n");
}

void print_nvm_userpage(uint8_t *page, uint8_t count){
	PRINT("idx: NVM Data\n");
	PRINT("   : B0 B1 B2 B3 \n");
	PRINT("   : ----------- \n");
    for (int i = 0; i < count; ){
        PRINT("%.2d : %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x \r\n",i, page[0+i], page[1+i], page[2+i], page[3+i]);
        i += 4;
    }
}

void print_nvm_TMpage(uint8_t *page, uint16_t count){
	PRINT("PG: idx: NVM TM Data\n");
	PRINT("   : B0 B1 B2 B3 B4 B5 B6 B7 \n");
	PRINT("   : ----------------------- \n");
    for (int i = 0; i < count; ){
        PRINT("%.2d : %.3d : %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x \r\n",(i/8),i, page[0+i], page[1+i], page[2+i], page[3+i],page[4+i],
        		page[5+i], page[6+i], page[7+i]);
        i += 8;
    }
}

void print_nvm_page(uint8_t *data, uint8_t pagenumber){
  
	if(pagenumber<100){
		PRINT(": Page %.2d\n", pagenumber);
	}else{
		PRINT(": Page %.3d\n", pagenumber);
	}
	//PRINT(": B0 B1 B2 B3 B4 B5 B6 B7 \n");
	//PRINT(": ----------------------- \n");

	PRINT(": %.2X %.2X %.2X %.2X \r\n",data[0], data[1], data[2], data[3]);
	PRINT("\n");
}

void print_nvm_page_with_offset(uint8_t *data, uint8_t pagenumber, uint8_t offset){
  

	PRINT("<< Page %.3d\n", pagenumber+offset);

	//PRINT(": B0 B1 B2 B3 B4 B5 B6 B7 \n");
	//PRINT(": ----------------------- \n");

	PRINT(": %.2X %.2X %.2X %.2X \r\n",data[0], data[1], data[2], data[3]);
	PRINT("\n");
}
void cls_screen(){
    printf("\x1b[2J\x1b[;H");/* \x1b[2J\x1b[;H - ANSI ESC sequence for clear screen */
}

void PRINT(const char *string,...){
//todo System porting required: implements platform print, if required.
    char buffer[256];
    va_list args;
    va_start(args, string);
    vsprintf(buffer, string, args);
    printf("%s", buffer);
    fflush(stdout); // Will now print everything in the stdout buffer
    va_end(args);
}

void PRINT_C(PRINT_COLOR COLOR, char *string,...){
	char buffer[256];

	switch(COLOR){
	case RED:
		printf("\033[1;31m");
		break;
	case GREEN:
		printf("\033[1;32m");
		break;
	case YELLOW:
		printf("\033[1;33m");
		break;
	case BLUE:
		printf("\033[1;34m");
		break;
	case MAGENTA:
		printf("\033[1;35m");
		break;
	case CYAN:
		printf("\033[1;36m");
		break;
	}

	va_list args;
	va_start(args, string);
	vsprintf(buffer, string, args);
	printf("%s", buffer);
	va_end(args);

	printf("\033[0m");
	fflush(stdout); // Will now print everything in the stdout buffer

}
#else
void PRINT(char *string,...){}
void PRINT_C(PRINT_COLOR COLOR, char *string,...){}
void cls_screen(void){}
void print_row_UID_table(uint8_t *UID){}
void print_odc(uint32_t *ODC){}
void print_puk(uint32_t *PUK){}
void print_nvm_userpage(uint8_t *page, uint8_t count){}
void print_nvm_TMpage(uint8_t *page, uint16_t count){}
void print_nvm_page(uint8_t *page, uint8_t pagenumber){}
#endif

/**
 * @brief This function implements displaying of byte array in hex format
 * @param  prgbBuf Pointer to the byte array to be displayed
 * @param wLen Length of byte array to be displayed
 */
void print_array(uint8_t* prgbBuf, uint16_t wLen)
{
	uint16_t wIndex;

	for(wIndex = 0; wIndex < wLen; wIndex++)
	{
        if ((wIndex % 10) == 0)
        {
            printf("\r\n");
        }
		if(*(prgbBuf+wIndex) < 16)
		{
			printf("0%X ", *(prgbBuf+wIndex));
		}
		else
		{
			printf("%X ", *(prgbBuf+wIndex));
		}
	}
	printf("\r\n");
}

void print_msg_digest(uint8_t* data, uint8_t len){
    char print[10];
    for (uint32_t i=0; i < len; i++)
    {
        if ((i % 10) == 0)
        {
            printf("\r\n");
        }
        sprintf(print,"0x%02X ", *(data+i));
        printf("%s", print);
    }
    printf("\r\n");
}

void PrintGf2nHex(uint32_t* data){
	for(uint8_t i =0;i < ARRAY_LEN(actual_degree);i++){
		PRINT("0x%.8X ",data[i]);
  }
	PRINT("\r\n");
}

/**
* @brief Sets the first number of bytes of the block of memory pointed by Pointer s to the specified value. Return: A pointer to the filled memory area s.
* @param s A pointer to the memory area to be filled.
* @param c The character to fill the memory area with.
* @param n The number of bytes to be filled.
*/
void *memset(void *s, int c, size_t n) {
  unsigned int index;
  unsigned char *memory = s, value = c;

  for (index = 0; index < n; index++)
    memory[index] = value;

  return (memory);
}

/**
* @brief Copies the values of num bytes from the location pointed to by source directly to the memory block pointed to by destination.
* @param dest Pointer to the destination array where the content is to be copied, type-casted to a pointer of type void*.
* @param src Pointer to the source of data to be copied, type-casted to a pointer of type const void*.
* @param count Number of bytes to copy.
*/
void *memcpy(void *dest, const void *src, size_t count) {
  char *dst8 = (char *)dest;
  char *src8 = (char *)src;

  while (count--) {
    *dst8++ = *src8++;
  }
  return dest;
}

/**
* @brief Compare the memory region. Returns 0 if the memory is the same.
* @param mem1 Pointer to the memory 1 region where memory is compared, type-casted to a pointer of type void*.
* @param mem2 Pointer to the memory 2 region where memory is compared, type-casted to a pointer of type const void*.
* @param n Number of bytes to compare.
*/
uint8_t memchk(const void *mem1, const void *mem2, size_t n) {
  if (n != 0) {
    const uint8_t *p1 = mem1, *p2 = mem2;

    do {
      if (*p1++ != *p2++)
        return (*--p1 - *--p2);
    } while (--n != 0);
  }
  return (0);
}

/**
* @brief Converts 8-bit data to 32-bit data memory type.
* @param dest Pointer to the 32-bit destination array where the content is to be copied, type-casted to a pointer of type void*.
* @param src Pointer to the 8-bit source of data to be copied, type-casted to a pointer of type const void*.
* @param n Number of 8-bit to convert.
*/
void convert8_to_32(uint32_t *dest, uint8_t *src, uint8_t n) {
  uint8_t ubIndex = 0;
  uint8_t dest_size = 0;

  if (n % 4 == 0) {
    dest_size = n / 4;
  } else {
    dest_size = (n / 4) + 1;
  }

  /*!< Convert uint8_t to uint32_t */
  for (ubIndex = 0; ubIndex < dest_size; ubIndex++) {
    dest[ubIndex] = (uint32_t)src[ubIndex * 4];
    dest[ubIndex] += (uint32_t)src[(ubIndex * 4) + 1] << 8;
    dest[ubIndex] += (uint32_t)src[(ubIndex * 4) + 2] << 16;
    dest[ubIndex] += (uint32_t)src[(ubIndex * 4) + 3] << 24;
  }
}

/**
* @brief Converts 32-bit data to 8-bit data memory type.
* @param dest Pointer to the 8-bit destination array where the content is to be copied, type-casted to a pointer of type void*.
* @param src Pointer to the 32-bit source of data to be copied, type-casted to a pointer of type const void*.
* @param n Number of 32-bit to convert.
*/
void convert32_to_8(uint8_t *dest, uint32_t *src, uint8_t n) {
  uint8_t ubIndex = 0;
  uint8_t dest_size = n;

  for (ubIndex = 0; ubIndex < dest_size; ubIndex++) {
    dest[ubIndex * 4] = (uint8_t)src[ubIndex];
    dest[(ubIndex * 4) + 1] = (uint8_t)(src[ubIndex] >> 8);
    dest[(ubIndex * 4) + 2] = (uint8_t)(src[ubIndex] >> 16);
    dest[(ubIndex * 4) + 3] = (uint8_t)(src[ubIndex] >> 24);
  }
}


/**
* @brief CRC8
*/
uint8_t CRC8(const uint8_t *data, uint32_t length)
{
   volatile uint8_t crc = 0x00;
   volatile uint8_t extract;
   volatile uint8_t sum;

   for(uint32_t i=0; i<length; i++)
   {
      extract = *data;
      for (uint8_t tempI = 8; tempI; tempI--)
      {
         sum = (crc ^ extract) & 0x01;
         crc >>= 1;
         if (sum){
            crc ^= 0x8C;
         }
         extract >>= 1;
      }
      data++;
   }
   return crc;
}
